#include "HOGcache.h"

HOG::HOGCache::HOGCache(CvSize _winSize, CvSize _blockSize, CvSize _blockStride, CvSize _cellSize, double _sigma, int _histogramNormType, double _L2HysThreshold):winSize(_winSize),blockSize(_blockSize),blockStride(_blockStride),cellSize(_cellSize),winSigma(_sigma),histogramNormType(_histogramNormType),L2HysThreshold(_L2HysThreshold)
{
  init();
}
HOG::HOGCache::~HOGCache() 
{
  if( imageGradient ) delete imageGradient ;
}
// 得到高斯的标准方差
float HOG::HOGCache::getWinSigma() 
{
  winSigma = winSigma >= 0 ? winSigma:(blockSize.width+blockSize.height)/8.;
  return winSigma ;
}
// 进行一些初始化工作和block的权值计算
void HOG::HOGCache::init()
{
  imageGradient = new ImageGradient ;
  if( imageGradient == NULL ) 
    {
      ERROR("can't create the ImageGradient Object!") ;
      exit(-1) ;
    }
  // 计算得到一个窗口windows中有多少个block 
  nblocks = cvSize((winSize.width-blockSize.width)/blockStride.width + 1 ,
		   (winSize.height-blockSize.height)/blockStride.height + 1) ; 
  // 进行block与cell的数据一致性 
  if( blockSize.width % cellSize.width || blockSize.height % cellSize.height ) 
    {
      ERROR("cellSize is not suit for blockSize!") ;
      exit(-1) ;
    }
  ncells = cvSize(blockSize.width/cellSize.width,blockSize.height/cellSize.width) ;
  int nbins = imageGradient->nbins ; // HOG方向的维度 
  blockHistogramSize = ncells.width * ncells.height * nbins ; // 得到一个block总共有多少维的HOG数据 2*2*9 = 36 ；
  int rawBlockSize = blockSize.width * blockSize.height ; 
  // 申请动态内存进行高斯权重的计算
  float ** weights = new float*[blockSize.height] ;
  for( int index = 0 ; index < blockSize.height ; ++ index )
    weights[index] = new float[blockSize.width] ;
  
  float sigma = getWinSigma() ;
  float scale = -1.f / (sigma*sigma*2) ;
  // 计算出一个block中各个像素点的高斯权重
  for( int index_rows = 0 ; index_rows < blockSize.height ; ++ index_rows )
    for( int index_cols = 0; index_cols  < blockSize.width ; ++ index_cols )
      {
	float drow = index_rows - (blockSize.height-1)*0.5f ;
	float dcol = index_cols - (blockSize.width-1)*0.5f ;
	weights[index_rows][index_cols] = std::exp((drow*drow+dcol*dcol)*scale) ;
      }

  blockData.resize(nblocks.width*nblocks.height) ;
  pixData.resize(rawBlockSize*3) ;

  // 计算参考的权重表
  // 通过对数据0-15进行相应偏移 映射到 -1 1 之间。使得4个cell的坐标变为 0 1 ;
  // 4个cell的横向中心坐标为3.5 和 11.5 通过 3.5 + 0.5 / 8 - 0.5 = 0 和 11.5+0.5 / 8 - 0.5 = 1 来进行坐标的映射
  count1 = count2 = count4 = 0 ;
  for( int index_row = 0 ; index_row < blockSize.height ; ++ index_row )
    for( int index_col = 0 ; index_col < blockSize.width ; ++ index_col )
      {
	PixData * data = NULL ;
	float cellX = (index_col+0.5)/cellSize.width - 0.5f ;
	float cellY = (index_row+0.5)/cellSize.width - 0.5f ;
	int icellX0 = cvFloor(cellX) ;
	int icellY0 = cvFloor(cellY) ;
	int icellX1 = icellX0 + 1 , icellY1 = icellY0 + 1 ;
	cellX -= icellX0 ; // 得到两个到相邻cell的权重
	cellY -= icellY0 ;
	
	/*
	  通过这种映射，可以很好的判断某个pix的所属区域和它对几个cell进行权值贡献。
	  通过对每一个cell进行4分使得16*16的block变成了
	   1  2  3  4
	   5  6  7  8 
	   9 10 11 12
	  13 14 15 16
	  16个块，其中 1，4，13，16只对本身所在的cell贡献权值
	  2 3 5 9 8 12 14 15 只对临近的两个cell贡献权值
	  6 7 10 11 最中间的4个像素块对4个cell都贡献权值
	  (icellX0,icellY0,icellX1,icellY1)
	  (-1,-1,0,0)+(-1,1,0,2)+(1,-1,2,0)+(1,1,2,2) => 1 4 13 16 四个角的4个像素点块，只对所在的cell贡献权值
	  (-1,0,0,1)+(0,-1,1,0)+(1,0,2,1)+(0,1,1,2) => 5 9 2 3 8 12 14 15 四个边界的中间部分对临近的两个cell贡献权值
	  (0,0,1,1) 6 7 10 11 对4个cell都贡献权值
	 */
	if( (unsigned)icellX0 < (unsigned)ncells.width && (unsigned)icellX1 < (unsigned)ncells.width )
	  {
	    // icellX0 == 0 
	    if( (unsigned)icellY0 < (unsigned)ncells.height && (unsigned)icellY1 < (unsigned)ncells.height )
	      {
		// icellY0 == 0 
		// 贡献4个cell的情况
		data  = &pixData[rawBlockSize*2+(count4++)] ;
		data->histOfs[0] = (icellY0*ncells.width+icellX0)*nbins ;
		data->histWeights[0] = (1.f-cellX)*(1.f-cellY) ;
		data->histOfs[1] = (icellY0*ncells.width+icellX1)*nbins ;
		data->histWeights[1] = cellX*(1.f-cellY) ;
		data->histOfs[2] = (icellY1*ncells.width+icellX0)*nbins ;
		data->histWeights[2] = (1.f-cellX)*cellY ;
		data->histOfs[3] = (icellY1*ncells.width+icellX1)*nbins ;
		data->histWeights[3] = cellX*cellY ;
	      }
	    else
	      {
		// 2个cell贡献权值
		data = &pixData[rawBlockSize+(count2++)] ; 
		if( (unsigned)icellY0 < (unsigned)ncells.height )
		  {
		    // Y0 = 1 Y1 =2 
		    icellY1 = icellY0 ; // 1 
		    cellY = 1.f - cellY ;
		  }
		data->histOfs[0] = (icellY1*ncells.width+icellX0)*nbins ;
		data->histWeights[0] = (1.f-cellX)*cellY ;
		data->histOfs[1] = (icellY1*ncells.width+icellX1)*nbins ;
		data->histWeights[1] = cellX*cellY ;
		data->histOfs[2] = data->histOfs[3] = 0 ;
		data->histWeights[2] = data->histWeights[3] = 0 ;
	      }
	  }
	else
	  {
	    // 调整col的贡献的cell的位置与权值
	    if( (unsigned)icellX0 < (unsigned)ncells.width )
	      {
		icellX1 = icellX0 ; // 1
		cellX = 1.f - cellX ;
	      }
	    if( (unsigned)icellY0 < (unsigned)ncells.height && (unsigned)icellY1 < (unsigned)ncells.height)
	      {
		// 贡献两个cell
		data = &pixData[rawBlockSize+(count2++)];
		data->histOfs[0] = (icellY0*ncells.width+icellX1)*nbins ;
		data->histWeights[0] = cellX * (1.f-cellY) ;
		data->histOfs[1] = (icellY1*ncells.width+icellX1)*nbins ;
		data->histWeights[1] = cellX * cellY ;
		data->histOfs[2] = data->histOfs[3] = 0 ;
		data->histWeights[2] = data->histWeights[3] = 0 ;
	      }
	    else
	      {
		// 贡献一个cell
		data = &pixData[count1++] ;
		if( (unsigned)icellY0 < (unsigned)ncells.height )
		  {
		    icellY1 = icellY0 ; // 1 ;
		    cellY = 1.f - cellY ;
		  }
		data->histOfs[0] = (icellY1*ncells.width+icellX1)*nbins ;
		data->histWeights[0] = cellX * cellY ;
		data->histOfs[1] = data->histOfs[2] = data->histOfs[3] = 0 ;
		data->histWeights[1] = data->histWeights[2] = data->histWeights[3] = 0 ;
	      }
	  }
	data->location.x = index_col ;
	data->location.y = index_row ;
	data->gradWeight = weights[index_row][index_col] ;
      }
  
  if( count1 + count2 + count4 != rawBlockSize ) ERROR("pix count is not equal total pixels in a blocks!")  ;

  for( int index = 0 ; index < count2 ; ++ index )
    pixData[count1+index] = pixData[index+rawBlockSize] ;
  for( int index = 0 ; index < count4 ; ++ index )
    pixData[count1+count2+index] = pixData[index+rawBlockSize*2] ; 
  
  count2 += count1 ;
  count4 += count2 ;
  /*
  for( int index = 0 ; index < count4 ; ++ index )
    {
      std::cout << "index:" << index << " Point:" << pixData[index].location.x << " " << pixData[index].location.y << std::endl ;
      std::cout << "histOfs: 0 " << pixData[index].histOfs[0] << " 1 " << pixData[index].histOfs[1] << " 2 " << pixData[index].histOfs[2] << " 3 " << pixData[index].histOfs[3] << std::endl ;
      std::cout << "histWeights: 0 " << pixData[index].histWeights[0] << " 1 " << pixData[index].histWeights[1] << " 2 " << pixData[index].histWeights[2] << " 3 " << pixData[index].histWeights[3] << std::endl ;
      std::cout << "gradWeight:" << pixData[index].gradWeight << std::endl ;
    }
  */
  // 初始化blockData
  for( int index_row = 0 ; index_row < nblocks.height ; ++ index_row )
    for( int index_col = 0 ; index_col < nblocks.width ; ++ index_col )
      {
	BlockData& data = blockData[index_row*nblocks.width+index_col] ;
	data.histOfs = (index_row*nblocks.width+index_col)*blockHistogramSize ;
	data.imgOffset = cv::Point(index_col*blockStride.width,index_row*blockStride.height) ;
      }

  // 对动态申请的内存进行释放
  for( int index = 0 ; index < blockSize.height ; ++ index )
    delete []weights[index] ;
  delete []weights ;

}
// 通过给出一个偏移pt， 然后得到一个block的hog特征36bins
const float * HOG::HOGCache::GetBlock(cv::Point pt, float * buf) 
{
  if( buf == NULL ) ERROR("block buf is NULL!") ;
  float * blockHist = buf ;
  
  CvSize imageSize = imageGradient->imageSize ;
  if( (unsigned)pt.x > (unsigned)(imageSize.width-blockSize.width) || (unsigned)pt.y > (unsigned)(imageSize.height-blockSize.height) )
    {
      WARNING("block is out of range of image!");
      return  NULL ;
    }
  const uchar* gradPtr = (uchar*)(grad.data) ;
  const uchar* qanglePtr = qangle.data ;

  for( int index = 0 ; index < blockHistogramSize ; ++ index )
    blockHist[index] = 0.f ;

  const PixData* _pixData = &pixData[0] ;

  for( int index = 0 ; index < count1 ; ++ index )
    {
      const PixData& pindex = _pixData[index] ; // get the pix struct ;
      cv::Point dstPoint = pindex.location + pt ;
      size_t gradOfs = dstPoint.y * grad.step + dstPoint.x*2*sizeof(float) ;
      size_t qangleOfs = dstPoint.y * qangle.step + dstPoint.x*2 ;
      const float* a = (float*)(gradPtr+gradOfs) ; // get the pix grad address ; a[0] is low angle's,grad a[1] is high angle's grad ;
      float w = pindex.gradWeight*pindex.histWeights[0] ; // get the pix one cell's weight ; 
      const uchar * h = qanglePtr+qangleOfs ; //  get the pix's angle ; h[0] is low angle; h[1] is high angle ;
      int h0 = h[0] , h1 = h[1] ; 
      float* hist  = blockHist + pindex.histOfs[0] ; // get the OFS of the pix's cell in the histbuff ; 
      float t0 = hist[h0] + a[0] * w ; 
      float t1 = hist[h1] + a[1] * w ;
      hist[h0] = t0 ; 
      hist[h1] = t1 ;
    }
  for( int index = count1 ; index < count2 ; ++ index )
    {
      const PixData& pindex = _pixData[index] ;
      cv::Point dstPoint = pindex.location + pt ;
      size_t gradOfs = dstPoint.y * grad.step + dstPoint.x*2*sizeof(float) ;
      size_t qangleOfs = dstPoint.y * qangle.step + dstPoint.x*2 ;
      const float* gradindex = (float*)(gradPtr+gradOfs) ; 
      float w,t0,t1,a0=gradindex[0],a1=gradindex[1] ;
      const uchar * angleindex = qanglePtr+qangleOfs ;
      int h0 = angleindex[0] , h1 = angleindex[1] ;
      
      float * hist = blockHist + pindex.histOfs[0] ;
      w = pindex.histWeights[0] * pindex.gradWeight ;
      t0 = hist[h0] + a0*w ;
      t1 = hist[h1] + a1*w ;
      hist[h0] = t0 ;
      hist[h1] = t1 ;
      
      hist = blockHist + pindex.histOfs[1] ;
      w = pindex.histWeights[1] * pindex.gradWeight ;
      t0 = hist[h0] + a0*w ;
      t1 = hist[h1] + a1*w ;
      hist[h0] = t0 ;
      hist[h1] = t1 ;
    }
  for( int index = count2 ; index < count4 ; ++ index )
    {
      const PixData& pindex = _pixData[index] ;
      cv::Point dstPoint = pt + pindex.location ;
      size_t gradOfs = dstPoint.y * grad.step + dstPoint.x*2*sizeof(float) ;
      size_t qangleOfs = dstPoint.y * qangle.step + dstPoint.x*2 ;
      const float* gradindex = (float*)(gradPtr+gradOfs) ;
      float w,t0,t1,a0 = gradindex[0] , a1 = gradindex[1] ;
      const uchar* angleindex = qanglePtr+qangleOfs ;
      int h0 = angleindex[0] , h1 = angleindex[1] ;
      float * hist = blockHist + pindex.histOfs[0] ;
      w = pindex.gradWeight * pindex.histWeights[0] ;
      t0 = hist[h0] + a0 * w ;
      t1 = hist[h1] + a1 * w ;
      hist[h0] = t0 ;
      hist[h1] = t1 ;

      hist = blockHist + pindex.histOfs[1] ;
      w = pindex.gradWeight * pindex.histWeights[1] ;
      t0 = hist[h0] + a0 * w ;
      t1 = hist[h1] + a1 * w ;
      hist[h0] = t0 ;
      hist[h1] = t1 ;

      hist = blockHist + pindex.histOfs[2]  ;
      w = pindex.gradWeight * pindex.histWeights[2] ;
      t0 = hist[h0] + a0*w ;
      t1 = hist[h1] + a1*w ;
      hist[h0] = t0 ;
      hist[h1] = t1 ;

      hist = blockHist + pindex.histOfs[2] ;
      w = pindex.gradWeight * pindex.histWeights[3] ;
      t0 = hist[h0] + a0 * w ;
      t1 = hist[h1] + a1 * w ;
      hist[h0] = t0 ;
      hist[h1] = t1 ;
    }
  normalizeBlockHistogram(blockHist) ;
  return blockHist ;
}
void HOG::HOGCache::normalizeBlockHistogram(float* _hist) const
{
  float* hist = &_hist[0] ;
  float sum = 0 ;

  for( int index = 0; index < blockHistogramSize ; ++ index )
      sum += hist[index] * hist[index] ;
  
  float scale = 1.f/(std::sqrt(sum)+0.1f*blockHistogramSize) ;
  float thresh = (float)L2HysThreshold ; 
  sum =  0 ; 
  for( int index = 0 ; index < blockHistogramSize ; ++ index )
    {
      hist[index] = std::min(hist[index]*scale,thresh) ;
      sum += hist[index]*hist[index] ;
    }
  scale = 1.f / (std::sqrt(sum)+1e-3f) ;
  for( int index = 0 ; index < blockHistogramSize ; ++ index )
    hist[index]*=scale ;
}
// 计算一个window的hog特征
void HOG::HOGCache::GetWindow(cv::Point pt,float * buf)
{
  float* windowHistBuf = buf ;
  if( windowHistBuf == NULL )
    {
      ERROR("window hog buffer is NULL!") ;
      exit(-1) ;
    }
  CvSize imageSize = imageGradient->imageSize;
  if( (unsigned)pt.x > (unsigned)(imageSize.width - winSize.width) || (unsigned)pt.y > (unsigned)(imageSize.height-winSize.height) )
    {
      ERROR("window is out of image!") ;
      exit(-1) ;
    }
  size_t blockNum = nblocks.width * nblocks.height ;  // 得到blockData的维数
  for( int index = 0 ; index < blockNum ; ++ index )
    {
      cv::Point pBlock = pt + blockData[index].imgOffset ;
      float* histPtr = windowHistBuf+blockData[index].histOfs ; 
      GetBlock(pBlock,histPtr) ;
    }
}
void HOG::HOGCache::calculateGradient(IplImage*_img)
{
  imageGradient->CalculateGradient(_img,grad,qangle) ;
}
// 预处理掉一些不可计算的hog windows点
void HOG::HOGCache::preprocessLocation(std::vector<cv::Point> &_winLocations)
{
  CvSize imageSize = imageGradient->imageSize;
  typedef std::vector<cv::Point>::iterator vectorIterType ;
  for( vectorIterType iter = _winLocations.begin() ; iter != _winLocations.end() ; )
    {
      int locationX = iter->x - (winSize.width>>2) ;
      int locationY = iter->y - (winSize.height>>2) ;
      locationX = locationX > 0 ? locationX : 0 ;
      locationY = locationY > 0 ? locationY : 0 ;
      if( (unsigned)iter->x > (unsigned)(imageSize.width-winSize.width) || (unsigned)iter->y > (unsigned)(imageSize.height-winSize.height) )
	iter = _winLocations.erase(iter) ;
      else iter ++ ;
    }
}
// img: 计算hog特征的图像
// HOGfeature: HOG特征
// locations: 需要计算的特征的所有窗口的起始坐标
int HOG::HOGCache::compute(IplImage* img, std::vector<float>& HOGfeature, std::vector<cv::Point>& locations)
{
  if( img == NULL ) 
    {
      ERROR("image is NULL !") ;
      exit(-1) ;
    }
  calculateGradient(img) ;
  preprocessLocation(locations) ;
  size_t nwindows = locations.size() ;
  size_t histSize = nblocks.width * nblocks.height * blockHistogramSize ; // windows中多少个block * 每个block有多少维的特征向量
  HOGfeature.resize(histSize*nwindows) ;
  
  for( size_t index = 0 ; index < nwindows ; ++ index )
    {
      cv::Point pw = locations[index];
      float* bufPtr = &HOGfeature[index*histSize]; 
      GetWindow(pw,bufPtr);
    }
  return histSize ;
}
